#include "zf_common_headfile.h"

volatile int16 encoder_data_quaddec = 0; // 编码器值
volatile uint16_t encoder_accu=0;

volatile float Err=0; // 摄像头误差

float sErr=0;
uint8_t timer_Zebra=0;
int16_t count_50ms=0;

pid_param_t Cascade_Pid;
pid_param_t Speed_Pid;

pd_param_t Cascade_Pd;
pd_param_t Speed_Pd;
pd_param_t Side_Cascade_Pd;


pid_param_t Gyroscope_Pid;
Set_Pid_Struct Set_Pid;              //pid系数结构体
pd_param_t My_pd;

uint8_t Car_Status = STOP; // 车辆状态

fifo_struct uart_data_fifo;
uint8       uart_get_data[64];                                                  // 串口接收数据缓冲区
uint8       fifo_get_data[64];                                                  // fifo 输出读出缓冲区

uint8       get_data = 0;                                                       // 接收数据变量
uint32      fifo_data_count = 0;                                                // fifo 数据个数

char buffer[1024];

void test_motor()
{
    uint16 duty1 = 500;
    uint16 duty2 = 500;
    uint16 duty3 = 500;
    uint16 duty4 = 500;
    int8_t menu=0;

    mytft_init();
    key_init(10);
    BLDC_init();
    battery_voltage_init();

//    motor_init();
//    handler_func(swap(&duty1,&duty2));
    while (1)
    {
        key_scanner();
        battery_voltage_get();
        if(key_get_state(KEY_3))
            if(++menu==4)menu=0;
        if(key_get_state(KEY_4))
            if(--menu==-1)menu=3;

        switch(menu)
        {
        case 0:
            if (key_get_state(KEY_1))
                duty1 += 10;
            if (key_get_state(KEY_2))
                duty1 -= 10;
            break;
        case 1:
            if (key_get_state(KEY_1))
                duty2 += 10;
            if (key_get_state(KEY_2))
                duty2 -= 10;
            break;
        case 2:
            if (key_get_state(KEY_1))
                duty3 += 10;
            if (key_get_state(KEY_2))
                duty3 -= 10;
            break;
        case 3:
            if (key_get_state(KEY_1))
                duty4 += 10;
            if (key_get_state(KEY_2))
                duty4 -= 10;
            break;
        }
//        motoir(duty1, 0, duty2, 0);
        tft180_show_string(95, 0, "vol");
        tft180_show_float(95, 13, battery.voltage / 1000.0, 2, 1);
        BLDC(duty1,duty2,duty2,duty1);
        BLDE_Side(duty3,duty4);
        tft180_show_string(0, 8*0, "BLDC:");tft180_show_string(6*7, 8*0, "menu:");tft180_show_int(6*12, 8*0, menu,1);
        tft180_show_string(0, 8*1, "pwm1:");tft180_show_int(4 * 8, 8*1, duty1, 4);
        tft180_show_string(0, 8*2, "pwm2:");tft180_show_int(4 * 8, 8*2, duty2, 4);

        tft180_show_string(0, 8*4, "BLDE_Side:");
        tft180_show_string(0, 8*5, "pwm1:");tft180_show_int(4 * 8, 8*5, duty3, 5);
        tft180_show_string(0, 8*6, "pwm2:");tft180_show_int(4 * 8, 8*6, duty4, 5);
    }
}


void uart_rx_interrupt_handler (void)
{
//    get_data = uart_read_byte(UART_INDEX);                                      // 接收数据 while 等待式 不建议在中断使用
    uart_query_byte(UART_2, &get_data);                                     // 接收数据 查询式 有数据会返回 TRUE 没有数据会返回 FALSE
}

void text_ble()
{
    uart_init(UART_2,9600,UART2_MAP1_TX_D5,UART2_MAP1_RX_D6);
    uart_rx_interrupt(UART_2, ZF_ENABLE);                                   // 开启 UART_INDEX 的接收中断
    interrupt_set_priority(USART2_IRQn, (0<<5) || 1);                         // 设置对应 UART_INDEX 的中断抢占优先级0，子优先级1

    while(1)
    {
           uart_write_string(UART_2, "UART get data:");                     // 输出测试信息
           uart_write_string(UART_2, "\r\n");                               // 输出测试信息
     }
     system_delay_ms(50);
//    mytft_init();
//    while (dl1b_init())
//        tft180_show_string(0, 0, "INIT FALL!");
//
//    while (1)
//    {
//        dl1b_get_distance();
//        tft180_show_int(0, 0, dl1b_distance_mm, 4);
//        system_delay_ms(5);
//    }
}

/*****储存发送行列标号的数组*****/
char ZF[105][4]={"0 \0","1 \0","2 \0","3 \0","4 \0","5 \0","6 \0","7 \0","8 \0","9 \0","10\0",
                  "11\0","12\0","13\0","14\0","15\0","16\0","17\0","18\0","19\0","20\0",
                  "21\0","22\0","23\0","24\0","25\0","26\0","27\0","28\0","29\0","30\0",
                  "31\0","32\0","33\0","34\0","35\0","36\0","37\0","38\0","39\0","40\0",
                  "41\0","42\0","43\0","44\0","45\0","46\0","47\0","48\0","49\0","50\0",
                  "51\0","52\0","53\0","54\0","55\0","56\0","57\0","58\0","59\0","60\0",
                  "61\0","62\0","63\0","64\0","65\0","66\0","67\0","68\0","69\0","70\0",
                  "71\0","72\0","73\0","74\0","75\0","76\0","77\0","78\0","79\0","80\0",
                  "81\0","82\0","83\0","84\0","85\0","86\0","87\0","88\0","89\0","90\0",
                  "91\0","92\0","93\0","94\0","95\0","96\0","97\0","98\0","99\0","  \0",};

void Uart_TX_Picture(void)//串口发送摄像头二值化图片函数
{
    uart_init(UART_3,115200,UART3_MAP0_TX_B10,UART3_MAP0_RX_B11);       // 初始化串口0 波特率115200 发送引脚使用P14_0 接收引脚使用P14_1//串口初始化函数
//    CAMERA_Init(50); /* 摄像头初始化 */
//    OLED_Init();//OLED初始化函数
    kamera_init();
//    mytft_init();

    while(1)
    {
        //1.图片采集更新
        if (mt9v03x_finish_flag)
        {
            Get_Use_Image(0); // 裁剪图像
            Threshold=My_Adapt_Threshold(mt9v03x_image[0],MT9V03X_W, MT9V03X_H);//大津计算阈值
            Image_Binarization(Threshold);//图像二值化
            mt9v03x_finish_flag = 0;/* 清除摄像头采集完成标志位  如果不清除，则不会再次采集数据 */

//            tft180_show_gray_image(0, 0, TFT18_Bin_data[0], LCDW, LCDH, LCDW,LCDH, Threshold);//TFT屏显示灰度图  显示原始图像  显示原始图像96/60
        }

        //2.图片串口发送
        //将二值化后的数组Bin_Image[LCDH][LCDW]上报给上位机
            for(int i=0; i<10; i++)
            {
                switch(i)
                {
                    case 0:uart_write_string(UART_3,"0 1 2 3 4 5 6 7 8 9 ");break;//发送列标号
                    case 1:uart_write_string(UART_3,"10111213141516171819");break;
                    case 2:uart_write_string(UART_3,"20212223242526272829");break;
                    case 3:uart_write_string(UART_3,"30313233343536373839");break;
                    case 4:uart_write_string(UART_3,"40414243444546474849");break;
                    case 5:uart_write_string(UART_3,"50515253545556575859");break;
                    case 6:uart_write_string(UART_3,"60616263646566676869");break;
                    case 7:uart_write_string(UART_3,"70717273747576777879");break;
                    case 8:uart_write_string(UART_3,"80818283848586878889");break;
                    case 9:uart_write_string(UART_3,"90919293");break;
                }
            }

            uart_write_string(UART_3,"\r\n");//换行
            for(int i=0; i<60; i++)
            {
                for(int j=0; j<94; j++)
                {

                    uart_write_byte(UART_3,TFT18_Bin_data[i][j]+48);//发送0或1字符给上位机
                    uart_write_string(UART_3," ");
                    if(j==93)
                    {
                        uart_write_string(UART_3,ZF[i]);//发送行标号
                        uart_write_string(UART_3," \r\n");//换行
                    }
                }
            }
            uart_write_string(UART_3,"********************************************************************\r\n");//发送分界线字符串到上位机
//            system_delay_ms(5);
    }
}

int main(void)
{
    clock_init(SYSTEM_CLOCK_144M);
    debug_init();
//    text_ble();
    test_motor();
//    Uart_TX_Picture();

    key_init(10);
    var_init(); //变量初始化
    battery_voltage_init();
    gpio_init(BEEF, GPO, 0, GPO_PUSH_PULL);
    gpio_init(LED1, GPO, 0, GPO_PUSH_PULL);
    gpio_init(LED2, GPO, 0, GPO_PUSH_PULL);

    mytft_init();
    BLDC_init();
//    motor_init();
    encoder_init();
    kamera_init();
    My_Pid_Init();
    imu660_init();
    my_timer_init();
    tft180_clear();
    Read_Flash();

//    timer_init(TIM5_PIT,TIMER_FUNCTION_TIMER);
    while (1)
    {
//        timer_clear(TIM5_PIT);
//        timer_start(TIM5_PIT);
        if (mt9v03x_finish_flag)
        {
            mt9v03x_finish_flag = 0;
        }
        Get_Use_Image(0); // 裁剪图像

        Threshold=My_Adapt_Threshold(mt9v03x_image[0],MT9V03X_W, MT9V03X_H);//大津计算阈值
        Image_Binarization(Threshold);//图像二值化
//        tft180_show_gray_image(0, 0, TFT18_Bin_data[0],LCDW, LCDH ,LCDW, LCDH ,Threshold); // 二值化图像到屏幕
        Longest_White_Column(); //最长白列巡线
        Zebra_Stripes_Detect(); //斑马线检测
        Barrier_Detect();       //避障检测
        Straight_Detect();      //直道检测
        Island_Detect();        //环岛检测
        Cross_Detect();         //十字检测
        Err = Err_Sum();
        sErr = Err_Sum_side();
        if(menu==MENU_NUM)
        {
            gpio_set_level(LED1,1);
            gpio_set_level(LED2,0);
        }
        else
        {
            gpio_set_level(LED1,0);
            gpio_set_level(LED2,0);
        }

        if(!key_get_state(P4))
            display();
//        tft180_show_int(0,13*10,timer_get(TIM5_PIT),5);


    }
}
